#!/bin/bash
# USB相机管理工具（支持启动/停止/状态检查）
# 用法: ./usb_camera_manager.sh [start|stop|restart|status]

# ================= 配置区 =================
DEVICE="/dev/video0"           # 相机设备路径
WIDTH="640"                    # 图像宽度
HEIGHT="480"                   # 图像高度
FPS="30"                       # 帧率
ROS_VERSION="1"                # 1=ROS1, 2=ROS2
# ROS1参数
ROS1_PKG="usb_cam"             # ROS1包名
ROS1_LAUNCH="usb_cam-test.launch"  # 启动文件
# ROS2参数
ROS2_PKG="cv_camera"           # ROS2包名
ROS2_NODE="cv_camera_node"     # 节点名称
# ==========================================

# 检查ROS环境
check_ros_env() {
  if [ "$ROS_VERSION" = "1" ]; then
    if ! command -v roslaunch &> /dev/null; then
      echo "[ERROR] ROS1未安装或未source！请执行: source /opt/ros/noetic/setup.bash"
      exit 1
    fi
  else
    if ! command -v ros2 &> /dev/null; then
      echo "[ERROR] ROS2未安装或未source！请执行: source /opt/ros/${ROS_DISTRO}/setup.bash"
      exit 1
    fi
  fi
}

# 检查相机设备
check_camera() {
  if [ ! -e "$DEVICE" ]; then
    echo "[ERROR] 相机设备 $DEVICE 不存在！"
    echo "可用设备:"
    ls /dev/video* 2>/dev/null || echo "未检测到任何相机"
    exit 1
  fi
}

# 启动相机
start_camera() {
  check_ros_env
  check_camera

  if [ "$(get_status)" = "running" ]; then
    echo "[INFO] 相机已在运行 (PID: $(cat /tmp/usb_camera.pid))"
    return
  fi

  echo "启动USB相机 (设备: $DEVICE ${WIDTH}x${HEIGHT}@${FPS}fps)..."

  if [ "$ROS_VERSION" = "1" ]; then
    roslaunch $ROS1_PKG $ROS1_LAUNCH \
      video_device:="$DEVICE" \
      image_width:="$WIDTH" \
      image_height:="$HEIGHT" \
      framerate:="$FPS" \
      pixel_format:="yuyv" \
      camera_name:="usb_cam" \
      > /dev/null 2>&1 &
  else
    ros2 run $ROS2_PKG $ROS2_NODE \
      --ros-args \
      -p device_id:="$(basename $DEVICE | sed 's/video//')" \
      -p width:="$WIDTH" \
      -p height:="$HEIGHT" \
      -p rate:="$FPS" \
      > /dev/null 2>&1 &
  fi

  PID=$!
  echo $PID > /tmp/usb_camera.pid
  echo "[SUCCESS] 已启动 (PID: $PID)"
}

# 停止相机
stop_camera() {
  if [ -f "/tmp/usb_camera.pid" ]; then
    PID=$(cat /tmp/usb_camera.pid)
    echo "停止相机进程 (PID: $PID)..."
    kill -INT $PID
    rm -f /tmp/usb_camera.pid
    sleep 1

    if ps -p $PID > /dev/null; then
      echo "[WARNING] 正常停止失败，尝试强制终止..."
      kill -9 $PID
    fi
    echo "[SUCCESS] 已停止"
  else
    echo "[INFO] 未检测到运行中的相机进程"
  fi
}

# 获取状态
get_status() {
  if [ -f "/tmp/usb_camera.pid" ]; then
    PID=$(cat /tmp/usb_camera.pid)
    if ps -p $PID > /dev/null; then
      echo "running"
      return
    else
      rm -f /tmp/usb_camera.pid
    fi
  fi
  echo "stopped"
}

# 主逻辑
case "$1" in
  start)
    start_camera
    ;;
  stop)
    stop_camera
    ;;
  restart)
    stop_camera
    sleep 2
    start_camera
    ;;
  status)
    status=$(get_status)
    if [ "$status" = "running" ]; then
      echo "状态: 运行中 (PID: $(cat /tmp/usb_camera.pid))"
    else
      echo "状态: 已停止"
    fi
    ;;
  *)
    echo "用法: $0 {start|stop|restart|status}"
    exit 1
esac

exit 0